package legoproject;


import legoproject.Behaviors.Behavior;
import lejos.nxt.*;
import java.util.ArrayList;
import lejos.util.*;
import lejos.util.Stopwatch;

import legoproject.TestStubs.*;

public class GarbageCollector {
    // Testing
    /*
    public static final PrintReporter reporter = new PrintReporter("Master");
    public static final TouchSensorTestStub rightTouchSensor = new TouchSensorTestStub("rightTouchSensor", reporter);
    public static final TouchSensorTestStub leftTouchSensor = new TouchSensorTestStub("leftTouchSensor", reporter);
    public static final LightSensorTestStub lightSensor = new LightSensorTestStub("lightSensor", reporter);
    public static final UltrasonicSensorTestStub ultrasonicSensor = new UltrasonicSensorTestStub("ultrasonicSensor", reporter);
    public static final MotorPortTestStub rightMotorPort = new MotorPortTestStub();
    public static final MotorPortTestStub leftMotorPort = new MotorPortTestStub();
    public static final MotorTestStub rightMotor = new MotorTestStub();
    public static final MotorTestStub leftMotor = new MotorTestStub();
    public static final EscapeButtonTestStub escapeButton = new EscapeButtonTestStub("escapeButton", reporter);
    public static final BluetoothManagerTestStub bluetoothManager = new BluetoothManagerTestStub(reporter);
 */
 // Deployment
  
    public static final TouchSensor rightTouchSensor = new TouchSensor(SensorPort.S1);
    public static final TouchSensor leftTouchSensor = new TouchSensor(SensorPort.S4);
    public static final LightSensor lightSensor = new LightSensor(SensorPort.S3);
    public static final UltrasonicSensor ultrasonicSensor = new UltrasonicSensor(SensorPort.S2);
    public static final MotorPort rightMotorPort = MotorPort.A;
    public static final MotorPort leftMotorPort = MotorPort.B;
    public static final Motor rightMotor = Motor.A;
    public static final Motor leftMotor = Motor.B;
    public static final Button escapeButton = lejos.nxt.Button.ESCAPE;
    public static final Button leftButton = lejos.nxt.Button.LEFT;
    public static final StatusReporter reporter = new NXTReporter();
    //public static final StatusReporter reporter = new SilentReporter();
    public static final BluetoothManager bluetoothManager = new BluetoothManager();

    
    public ArrayList instructions = new ArrayList(); // This list is used by the motorController
    // when it runs
    private Behavior[] behaviors;
    private Behavior currentBehavior = null;
    private boolean controllerRunning = false;
    private Thread controllerThread = null;
    private int sleepTime = 80;
    public static boolean garbageInGrab = false;
    
    public static final int blackThreshold = lightSensor.readValue() - 10;

    public GarbageCollector(Behavior[] behaviors) {
        this.behaviors = behaviors;
        bluetoothManager.init();
        reporter.nameStat(0, "US");
        reporter.nameStat(1, "Light");
    }




    public void start() {
        Behavior nextBehavior = null;
        while (true) {
            nextBehavior = findBehavior();
            if (!controllerRunning || findPriority(nextBehavior) < findPriority(currentBehavior)) {
                if (controllerThread != null) {
                    controllerThread.interrupt();
                }
                while (controllerRunning) {
                    Thread.yield();
                }
                instructions = nextBehavior.behave();
                reporter.behaviorChange(nextBehavior, currentBehavior);
                reporter.updateLCD();
                currentBehavior = nextBehavior;
                controllerRunning = true;
                controllerThread = new MotorController(this);
                System.gc();
                controllerThread.start();
            }
            try {
                Thread.sleep(sleepTime);
            } catch (Exception e) {
            }
        }
    }

    /**
     * Is to be called by MotorController wheh current behaviors instructions
     * are done
     */
    public void done() {
        controllerRunning = false;
    }

    private Behavior findBehavior() {
        for (int i = 0; i < behaviors.length; i++) {
            if (behaviors[i].condition()) {
                return behaviors[i];
            }
        }
        return null;
    }

    private int findPriority(Behavior behavior) {
        for (int i = 0; i < behaviors.length; i++) {
            if (behaviors[i] == behavior) {
                return i;
            }
        }
        throw new NullPointerException();
    }
}


